﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Text;
using System.Timers;
using System.Linq;
using System.Runtime.InteropServices;
using System.Management;
using SharpDX;

namespace Engine
{
    public static class Utils
    {
        /// <summary>
        /// Радианы в градусы
        /// </summary>
        /// <param name="rad">Радианы</param>
        /// <returns>градусы</returns>
        public static float Rad2Deg(float rad)
        {
            return (float)((rad * 180d) / Math.PI);
        }

        /// <summary>
        /// Градусы в радианы
        /// </summary>
        /// <param name="deg">Градусы</param>
        /// <returns>Радианы</returns>
        public static float Deg2Rad(float deg)
        {
            return (float)((deg / 180d) * Math.PI);
        }

        /// <summary>
        /// Угол между векторами
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVec(Vector3 vec1, Vector3 vec2)
        {
            vec1.Normalize();
            vec2.Normalize();
            //return (float) Math.Acos(((vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z) * Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z))));
            double t4 = (vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z)) * (Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z));
            t4 = Math.Min(1, t4);
            t4 = Math.Max(-1, t4);
            return (float)Math.Acos(t4);
        }

        public static double AngleTwoVec(DVector3 vec1, DVector3 vec2)
        {
            vec1.Normalize();
            vec2.Normalize();
            //return Math.Acos(((vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z) * Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z))));
            double t4 = (vec1.X * vec2.X + vec1.Y * vec2.Y + vec1.Z * vec2.Z) / (Math.Sqrt(vec1.X * vec1.X + vec1.Y * vec1.Y + vec1.Z * vec1.Z)) * (Math.Sqrt(vec2.X * vec2.X + vec2.Y * vec2.Y + vec2.Z * vec2.Z));
            t4 = Math.Min(1, t4);
            t4 = Math.Max(-1, t4);
            return Math.Acos(t4);
        }

        /// <summary>
        /// Угол между векторами в плоскости X
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVecX(Vector3 vec_1, Vector3 vec_2)
        {
            Vector3 vec1_t = vec_1;
            Vector3 vec2_t = vec_2;
            vec1_t.Normalize();
            vec2_t.Normalize();
            float t = 1f;
            if ((vec1_t.X * vec2_t.Z - vec2_t.X * vec1_t.Z) < 0) t = -1;
            vec1_t.Y = 1f;
            vec2_t.Y = 1f;
            //             if (vec1.Length() > 0.001f) vec1.Normalize();
            //             else vec1 = new Vector3(1f, 0f, 0f);
            //             if (vec2.Length() > 0.001f) vec2.Normalize();
            //             else vec2 = new Vector3(1f, 0f, 0f);
            return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
        }

        /// <summary>
        /// Угол между векторами в плоскости Y
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVecY(Vector3 vec_1, Vector3 vec_2)
        {
            Vector3 vec1_t = vec_1;
            Vector3 vec2_t = vec_2;
            vec1_t.Normalize();
            vec2_t.Normalize();
            float t = 1f;
            if ((vec1_t.Z * vec2_t.Y - vec2_t.Z * vec1_t.Y) < 0) t = -1;
            vec1_t.X = 1f;
            vec2_t.X = 1f;
            //             if (vec1.Length() > 0.001f) vec1.Normalize();
            //             else vec1 = new Vector3(1f, 0f, 0f);
            //             if (vec2.Length() > 0.001f) vec2.Normalize();
            //             else vec2 = new Vector3(1f, 0f, 0f);
            return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
        }

        /// <summary>
        /// Угол между векторами в плоскости X
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVecX(DVector3 vec_1, DVector3 vec_2)
        {
            DVector3 vec1_t = vec_1;
            DVector3 vec2_t = vec_2;
            vec1_t.Normalize();
            vec2_t.Normalize();
            float t = 1;
            if ((vec1_t.X * vec2_t.Z - vec2_t.X * vec1_t.Z) < 0) t = -1;
            vec1_t.Y = 1f;
            vec2_t.Y = 1f;
            return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
        }

        /// <summary>
        /// Угол между векторами в плоскости Y
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVecY(DVector3 vec_1, DVector3 vec_2)
        {
            DVector3 vec1_t = vec_1;
            DVector3 vec2_t = vec_2;
            vec1_t.Normalize();
            vec2_t.Normalize();
            float t = 1;
            if ((vec1_t.Z * vec2_t.Y - vec2_t.Z * vec1_t.Y) < 0) t = -1;
            vec1_t.X = 1f;
            vec2_t.X = 1f;
            return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
        }

        /// <summary>
        /// Угол между векторами в плоскости Z
        /// </summary>
        /// <param name="vec1">Вектор1</param>
        /// <param name="vec2">Вектор2</param>
        /// <returns>Радианы</returns>
        public static float AngleTwoVecZ(DVector3 vec_1, DVector3 vec_2)
        {
            DVector3 vec1_t = vec_1;
            DVector3 vec2_t = vec_2;
            vec1_t.Normalize();
            vec2_t.Normalize();
            float t = 1;
            if ((vec1_t.Y * vec2_t.X - vec2_t.Y * vec1_t.X) < 0) t = -1;
            vec1_t.Z = 1f;
            vec2_t.Z = 1f;
            return t * (float)Math.Acos(((vec1_t.X * vec2_t.X + vec1_t.Y * vec2_t.Y + vec1_t.Z * vec2_t.Z) / (Math.Sqrt(vec1_t.X * vec1_t.X + vec1_t.Y * vec1_t.Y + vec1_t.Z * vec1_t.Z) * Math.Sqrt(vec2_t.X * vec2_t.X + vec2_t.Y * vec2_t.Y + vec2_t.Z * vec2_t.Z))));
        }

        /// <summary>
        /// Кватернион между двумя векторами
        /// </summary>
        /// <param name="UnitFrom">Начальный вектор</param>
        /// <param name="UnitTo">Конечный вектор</param>
        /// <returns>Кватернион</returns>
        public static Quaternion QuaternionFromTwoDirs(Vector3 UnitFrom, Vector3 UnitTo)
        {
            UnitFrom.Normalize();
            UnitTo.Normalize();
            Quaternion quat;
            float CosA = Vector3.Dot(UnitFrom, UnitTo);
            if (CosA < -0.99999f)
            {  // angle close to PI    ( can replaced by Bisect.lensquared() < 0.000001f ) ;
                Vector3 CrossVec = new Vector3(0, UnitFrom.X, -UnitFrom.Y); // cross with (1, 0, 0)
                if ((UnitFrom.Z * UnitFrom.Z) > (UnitFrom.Y * UnitFrom.Y))
                {
                    // if (0, 1, 0) Cross > (1, 0, 0) Cross
                    CrossVec = new Vector3(-UnitFrom.Z, 0, UnitFrom.X); // cross with (0 ,1, 0)
                }
                CrossVec.Normalize();
                quat = new Quaternion(CrossVec.X, CrossVec.Y, CrossVec.Z, 0.0f);
            }
            else
            {
                Vector3 Bisect = Vector3.Add(UnitFrom, UnitTo);
                Bisect.Normalize();
                Vector3 BCross = Vector3.Cross(UnitFrom, Bisect);
                quat = new Quaternion(BCross.X, BCross.Y, BCross.Z, Vector3.Dot(UnitFrom, Bisect));
            }
            return quat;
        }

        public static DQuaternion QuaternionFromTwoDirs(DVector3 UnitFrom, DVector3 UnitTo)
        {
            UnitFrom.Normalize();
            UnitTo.Normalize();
            DQuaternion quat;
            double CosA = DVector3.Dot(UnitFrom, UnitTo);
            if (CosA < -0.999999d)
            {  // angle close to PI    ( can replaced by Bisect.lensquared() < 0.000001f ) ;
                DVector3 CrossVec = new DVector3(0d, UnitFrom.X, -UnitFrom.Y); // cross with (1, 0, 0)
                if ((UnitFrom.Z * UnitFrom.Z) > (UnitFrom.Y * UnitFrom.Y))
                {
                    // if (0, 1, 0) Cross > (1, 0, 0) Cross
                    CrossVec = new DVector3(-UnitFrom.Z, 0, UnitFrom.X); // cross with (0 ,1, 0)
                }
                CrossVec.Normalize();
                quat = new DQuaternion(CrossVec.X, CrossVec.Y, CrossVec.Z, 0.0d);
            }
            else
            {
                DVector3 Bisect = DVector3.Add(UnitFrom, UnitTo);
                Bisect.Normalize();
                DVector3 BCross = DVector3.Cross(UnitFrom, Bisect);
                quat = new DQuaternion(BCross.X, BCross.Y, BCross.Z, DVector3.Dot(UnitFrom, Bisect));
            }
            return quat;
        }

        //Pitch Yaw Roll
        /// <summary>
        /// Кватернион в углы эйлера
        /// </summary>
        /// <param name="q1">Кватернион</param>
        /// <returns>Pitch, Yaw, Roll</returns>
        public static Vector3 QuatToEuler(Quaternion q1)
        {
            float test = q1.X * q1.Y + q1.Z * q1.W;
            float heading, attitude, bank;
            if (test > 0.499d)
            { // singularity at north pole
                heading = (float)(2d * Math.Atan2(q1.X, q1.W));
                attitude = (float)(Math.PI / 2d);
                bank = 0;
            }
            if (test < -0.499d)
            { // singularity at south pole
                heading = (float)(-2d * Math.Atan2(q1.X, q1.W));
                attitude = (float)(-Math.PI / 2d);
                bank = 0f;
            }
            float sqx = q1.X * q1.X;
            float sqy = q1.Y * q1.Y;
            float sqz = q1.Z * q1.Z;
            heading = (float)(Math.Atan2(2d * q1.Y * q1.W - 2d * q1.X * q1.Z, 1 - 2d * sqy - 2d * sqz));
            attitude = (float)(Math.Asin(2d * test));
            bank = (float)(Math.Atan2(2d * q1.X * q1.W - 2d * q1.Y * q1.Z, 1d - 2d * sqx - 2d * sqz));
            return new Vector3(bank, heading, attitude);
        }

        /// <summary>
        /// Матрица в углы эйлера (градусы)
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static Vector3 MatrixToEulerDeg(Matrix matrix)
        {
            double RD_TO_DEG = 180d / Math.PI;
            double h, p, b; // angles in degrees
            // extract pitch
            double sinP = -matrix.M23;
            if (sinP >= 1)
            {
                p = 90d;
            }       // pole
            else if (sinP <= -1d)
            {
                p = -90d;
            } // pole
            else
            {
                p = Math.Asin(sinP) * RD_TO_DEG;
            }

            // extract heading and bank
            if (sinP < -0.9999d || sinP > 0.9999d)
            { // account for small angle errors
                h = Math.Atan2(-matrix.M31, matrix.M11) * RD_TO_DEG;
                b = 0f;
            }
            else
            {
                h = Math.Atan2(matrix.M13, matrix.M33) * RD_TO_DEG;
                b = Math.Atan2(matrix.M21, matrix.M22) * RD_TO_DEG;
            }

            return new Vector3((float)h, (float)p, (float)b);
        }

        /// <summary>
        /// Матрица в углы эйлера (радианы)
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public static Vector3 MatrixToEulerRad(Matrix matrix)
        {
            double h, p, b; // angles in degrees
            // extract pitch
            double sinP = -matrix.M23;
            if (sinP >= 1d)
            {
                p = 90d;
            }       // pole
            else if (sinP <= -1d)
            {
                p = -90d;
            } // pole
            else
            {
                p = Math.Asin(sinP);
            }

            // extract heading and bank
            if (sinP < -0.9999d || sinP > 0.9999d)
            { // account for small angle errors
                h = Math.Atan2(-matrix.M31, matrix.M11);
                b = 0;
            }
            else
            {
                h = Math.Atan2(matrix.M13, matrix.M33);
                b = Math.Atan2(matrix.M21, matrix.M22);
            }

            return new Vector3((float)p, (float)h, (float)b);
        }

        /// <summary>
        /// Матрица между направлениями
        /// </summary>
        /// <param name="V1">Start</param>
        /// <param name="V2">Stop</param>
        /// <returns></returns>
        public static Matrix MatrixFromTwoVectors(Vector3 V2, Vector3 V1)
        {
            Matrix M = Matrix.Translation(new Vector3());
            Vector3 l = Vector3.Cross(V1, V2);
            float cf = Vector3.Dot(V1, V2); // cos(fi)
            float df = 1.0f / (1.0f + cf);
            Vector3 m = l * df;
            float a = l.X * m.Y;
            float b = l.Y * m.Z;
            float c = l.Z * m.X;
            M.M11 = l.X * m.X + cf;
            M.M22 = l.Y * m.Y + cf;
            M.M33 = l.Z * m.Z + cf;
            M.M12 = a - l.Z;
            M.M21 = a + l.Z;
            M.M13 = c + l.Y;
            M.M31 = c - l.Y;
            M.M23 = b - l.X;
            M.M32 = b + l.X;
            return M;
        }

        /// <summary>
        /// Матрица между направлениями
        /// </summary>
        /// <param name="V1">Start</param>
        /// <param name="V2">Stop</param>
        /// <returns></returns>
        public static DMatrix MatrixFromTwoVectors(DVector3 V2, DVector3 V1)
        {
            DMatrix M = DMatrix.CreateTranslation(new DVector3());
            DVector3 l = DVector3.Cross(V1, V2);
            double cf = DVector3.Dot(V1, V2); // cos(fi)
            double df = 1.0d / (1.0d + cf);
            DVector3 m = l * df;
            double a = l.X * m.Y;
            double b = l.Y * m.Z;
            double c = l.Z * m.X;
            M.M11 = l.X * m.X + cf;
            M.M22 = l.Y * m.Y + cf;
            M.M33 = l.Z * m.Z + cf;
            M.M12 = a - l.Z;
            M.M21 = a + l.Z;
            M.M13 = c + l.Y;
            M.M31 = c - l.Y;
            M.M23 = b - l.X;
            M.M32 = b + l.X;
            return M;
        }



        public static string GetProcessorID()
        {
            String processorID = "";
            ManagementObjectSearcher searcher = new ManagementObjectSearcher("Select * FROM WIN32_Processor");
            ManagementObjectCollection mObject = searcher.Get();
            foreach (ManagementObject obj in mObject)
            {
                processorID = obj["ProcessorId"].ToString();
            }
            return processorID;
        }

        public static bool[] GetProcessorIDInt()
        {
            bool[] Mass = new bool[128];


            string sProcessorID = GetProcessorID();

            string A0 = sProcessorID.Substring(0, 2);
            string A1 = sProcessorID.Substring(2, 2);
            string A2 = sProcessorID.Substring(4, 2);
            string A3 = sProcessorID.Substring(6, 2);

            string B0 = sProcessorID.Substring(8, 2);
            string B1 = sProcessorID.Substring(10, 2);
            string B2 = sProcessorID.Substring(12, 2);
            string B3 = sProcessorID.Substring(14, 2);

            byte a0 = byte.Parse(A0, System.Globalization.NumberStyles.HexNumber);
            byte a1 = byte.Parse(A1, System.Globalization.NumberStyles.HexNumber);
            byte a2 = byte.Parse(A2, System.Globalization.NumberStyles.HexNumber);
            byte a3 = byte.Parse(A3, System.Globalization.NumberStyles.HexNumber);

            return Mass;
        }

        public static double DistanceLineSegmentToPoint(DVector3 A, DVector3 B, DVector3 p)
        {
            //get the normalized line segment vector
            DVector3 v = B - A;
            v.Normalize();

            //determine the point on the line segment nearest to the point p
            double distanceAlongLine = DVector3.Dot(p, v) - DVector3.Dot(A, v);
            DVector3 nearestPoint;
            if (distanceAlongLine < 0)
            {
                //closest point is A
                nearestPoint = A;
            }
            else if (distanceAlongLine > DVector3.Distance(A, B))
            {
                //closest point is B
                nearestPoint = B;
            }
            else
            {
                //closest point is between A and B... A + d  * ( ||B-A|| )
                nearestPoint = A + distanceAlongLine * v;
            }

            //Calculate the distance between the two points
            double actualDistance = DVector3.Distance(nearestPoint, p);
            return actualDistance;
        }

        public static float DistanceLineSegmentToPoint(Vector3 A, Vector3 B, Vector3 p)
        {
            //get the normalized line segment vector
            Vector3 v = B - A;
            v.Normalize();

            //determine the point on the line segment nearest to the point p
            float distanceAlongLine = Vector3.Dot(p, v) - Vector3.Dot(A, v);
            Vector3 nearestPoint;
            if (distanceAlongLine < 0)
            {
                //closest point is A
                nearestPoint = A;
            }
            else if (distanceAlongLine > Vector3.Distance(A, B))
            {
                //closest point is B
                nearestPoint = B;
            }
            else
            {
                //closest point is between A and B... A + d  * ( ||B-A|| )
                nearestPoint = A + distanceAlongLine * v;
            }

            //Calculate the distance between the two points
            float actualDistance = Vector3.Distance(nearestPoint, p);
            return actualDistance;
        }

        /// <summary>
        /// Проверка на пересечение BB лучем
        /// </summary>
        /// <param name="VminB"></param>
        /// <param name="VmaxB"></param>
        /// <param name="Vorigin"></param>
        /// <param name="Vdir"></param>
        /// <param name="Vcoord"></param>
        /// <returns></returns>
        public static bool HitBoundingBox(DVector3 VminB, DVector3 VmaxB, DVector3 Vorigin, DVector3 Vdir, out DVector3 Vcoord)
        {
            Vcoord = new DVector3();
            double[] minB = new double[3] { VminB.X, VminB.Y, VminB.Z };
            double[] maxB = new double[3] { VmaxB.X, VmaxB.Y, VmaxB.Z };
            double[] origin = new double[3] { Vorigin.X, Vorigin.Y, Vorigin.Z };
            double[] dir = new double[3] { Vdir.X, Vdir.Y, Vdir.Z };
            double[] coord = new double[3] { 0, 0, 0 };
            bool inside = true;
            int[] quadrant = new int[3];
            int i;
            int whichPlane;
            double[] maxT = new double[3];
            double[] candidatePlane = new double[3];
            for (i = 0; i < 3; i++)
                if (origin[i] < minB[i])
                {
                    quadrant[i] = 1;
                    candidatePlane[i] = minB[i];
                    inside = false;
                }
                else
                    if (origin[i] > maxB[i])
                    {
                        quadrant[i] = 0;
                        candidatePlane[i] = maxB[i];
                        inside = false;
                    }
                    else quadrant[i] = 2;
            if (inside)
            {
                coord = origin;
                Vcoord = new DVector3(coord[0], coord[1], coord[2]);
                return true;
            }
            for (i = 0; i < 3; i++) if (quadrant[i] != 2 && dir[i] != 0) maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
                else maxT[i] = -1d;
            whichPlane = 0;
            for (i = 1; i < 3; i++) if (maxT[whichPlane] < maxT[i]) whichPlane = i;
            if (maxT[whichPlane] < 0) return false;
            for (i = 0; i < 3; i++)
                if (whichPlane != i)
                {
                    coord[i] = origin[i] + maxT[whichPlane] * dir[i];
                    if (coord[i] < minB[i] || coord[i] > maxB[i])
                        return false;
                }
                else
                {
                    coord[i] = candidatePlane[i];
                }
            Vcoord = new DVector3(coord[0], coord[1], coord[2]);
            return true;
        }

        /// <summary>
        /// Проверка на пересечение BB лучем
        /// </summary>
        /// <param name="VminB"></param>
        /// <param name="VmaxB"></param>
        /// <param name="Vorigin"></param>
        /// <param name="Vdir"></param>
        /// <param name="Vcoord"></param>
        /// <returns></returns>
        public static bool HitBoundingBox(Vector3 VminB, Vector3 VmaxB, Vector3 Vorigin, Vector3 Vdir, out Vector3 Vcoord)
        {
            Vcoord = new Vector3();
            float[] minB = new float[3] { VminB.X, VminB.Y, VminB.Z };
            float[] maxB = new float[3] { VmaxB.X, VmaxB.Y, VmaxB.Z };
            float[] origin = new float[3] { Vorigin.X, Vorigin.Y, Vorigin.Z };
            float[] dir = new float[3] { Vdir.X, Vdir.Y, Vdir.Z };
            float[] coord = new float[3] { 0, 0, 0 };
            bool inside = true;
            int[] quadrant = new int[3];
            int i;
            int whichPlane;
            float[] maxT = new float[3];
            float[] candidatePlane = new float[3];
            for (i = 0; i < 3; i++)
                if (origin[i] < minB[i])
                {
                    quadrant[i] = 1;
                    candidatePlane[i] = minB[i];
                    inside = false;
                }
                else
                    if (origin[i] > maxB[i])
                    {
                        quadrant[i] = 0;
                        candidatePlane[i] = maxB[i];
                        inside = false;
                    }
                    else quadrant[i] = 2;
            if (inside)
            {
                coord = origin;
                Vcoord = new Vector3(coord[0], coord[1], coord[2]);
                return true;
            }
            for (i = 0; i < 3; i++) if (quadrant[i] != 2 && dir[i] != 0) maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
                else maxT[i] = -1f;
            whichPlane = 0;
            for (i = 1; i < 3; i++) if (maxT[whichPlane] < maxT[i]) whichPlane = i;
            if (maxT[whichPlane] < 0) return false;
            for (i = 0; i < 3; i++)
                if (whichPlane != i)
                {
                    coord[i] = origin[i] + maxT[whichPlane] * dir[i];
                    if (coord[i] < minB[i] || coord[i] > maxB[i])
                        return false;
                }
                else
                {
                    coord[i] = candidatePlane[i];
                }
            Vcoord = new Vector3(coord[0], coord[1], coord[2]);
            return true;
        }

        /// <summary>
        /// Проверка на пересечение BB лучем
        /// </summary>
        /// <param name="VminB"></param>
        /// <param name="VmaxB"></param>
        /// <param name="Vorigin"></param>
        /// <param name="Vdir"></param>
        /// <param name="Vcoord"></param>
        /// <returns></returns>
        public static bool HitBoundingBox(Vector3 VminB, Vector3 VmaxB, Vector3 Vorigin, Vector3 Vdir)
        {
            float[] minB = new float[3] { VminB.X, VminB.Y, VminB.Z };
            float[] maxB = new float[3] { VmaxB.X, VmaxB.Y, VmaxB.Z };
            float[] origin = new float[3] { Vorigin.X, Vorigin.Y, Vorigin.Z };
            float[] dir = new float[3] { Vdir.X, Vdir.Y, Vdir.Z };
            float[] coord = new float[3] { 0, 0, 0 };
            bool inside = true;
            int[] quadrant = new int[3];
            int i;
            int whichPlane;
            float[] maxT = new float[3];
            float[] candidatePlane = new float[3];
            for (i = 0; i < 3; i++)
                if (origin[i] < minB[i])
                {
                    quadrant[i] = 1;
                    candidatePlane[i] = minB[i];
                    inside = false;
                }
                else
                    if (origin[i] > maxB[i])
                    {
                        quadrant[i] = 0;
                        candidatePlane[i] = maxB[i];
                        inside = false;
                    }
                    else quadrant[i] = 2;
            if (inside)
            {
                coord = origin;
                return true;
            }
            for (i = 0; i < 3; i++) if (quadrant[i] != 2 && dir[i] != 0) maxT[i] = (candidatePlane[i] - origin[i]) / dir[i];
                else maxT[i] = -1f;
            whichPlane = 0;
            for (i = 1; i < 3; i++) if (maxT[whichPlane] < maxT[i]) whichPlane = i;
            if (maxT[whichPlane] < 0) return false;
            for (i = 0; i < 3; i++)
                if (whichPlane != i)
                {
                    coord[i] = origin[i] + maxT[whichPlane] * dir[i];
                    if (coord[i] < minB[i] || coord[i] > maxB[i])
                        return false;
                }
                else
                {
                    coord[i] = candidatePlane[i];
                }
            return true;
        }

        // вычисление точки O пересечения прямой P1P2 и плоскости треугольника abc, лежащей внутри треугольника
        // возвращает растояние от P1 до точки пронзания вдоль вектора P1P2. 
        // Если возвращает NAN (не число) - значит такой точки нет
        // и координаты самой точки O
        /// <summary>
        /// Пересечение трингла прямой
        /// </summary>
        /// <param name="dist">растояние от P1 до точки пронзания вдоль вектора P1P2</param>
        /// <param name="O">координаты точки пересечения</param>
        /// <param name="N">нормаль</param>
        /// <param name="A">А</param>
        /// <param name="B">В</param>
        /// <param name="C">С</param>
        /// <param name="P1">Р1</param>
        /// <param name="P2">Р2</param>
        /// <returns>есть пересечение</returns>
        public static bool GetInterceptPoint(out Vector3 O, out Vector3 N, Vector3 A, Vector3 B, Vector3 C, Vector3 P1, Vector3 P2)
        {
            O = new Vector3();

            Vector3 V = P1 - P2;     // Направление прямой P1P2
            float Vdist = V.Length();
            Vector3 AB = (B - A);
            Vector3 AC = (C - A);
            Vector3 BC = (C - B);
            Vector3 BA = (A - B);
            N = Vector3.Cross(AB, AC);     // Нормаль к пов-ти
            if (Vector3.Dot(N, V) < 0)
            {
                N = Vector3.Cross(AC, AB);
            }

            float vn = Vector3.Dot(V, N);          // проекция V на N
            if (vn == 0) return false;      // нет пересечения (прямая паралельна плоскости)

            float t = -Vector3.Dot((P1 - A), N / vn);
            O = P1 + V * t;

            Vector3 NAB = Vector3.Cross(N, AB);  // нормаль к плоскости NAB
            Vector3 NAC = Vector3.Cross(N, AC);  // нормаль к плоскости NAC
            Vector3 NBC = Vector3.Cross(N, BC);  // нормаль к плоскости NBC

            Vector3 AO = (O - A);
            Vector3 BO = (O - B);

            float acnab = Vector3.Dot(AC, NAB);  // проекция AC на NAB
            float aonab = Vector3.Dot(AO, NAB);  // проекция AO на NAB

            float acnac = Vector3.Dot(AB, NAC);  // проекция AB на NAC
            float aonac = Vector3.Dot(AO, NAC);  // проекция AO на NAC

            float acnbc = Vector3.Dot(BA, NBC);  // проекция BA на NBC
            float bonbc = Vector3.Dot(BO, NBC);  // проекция BO на NBC

            if (Math.Sign(acnab) == Math.Sign(aonab) && Math.Sign(acnac) == Math.Sign(aonac) && Math.Sign(acnbc) == Math.Sign(bonbc)) return t <= Vdist;

            // принадлежность сторонам треугольника
            if (Math.Sign(aonab) == 0 && AO.Length() <= AB.Length()) return t <= Vdist;
            if (Math.Sign(aonac) == 0 && AO.Length() <= AC.Length()) return t <= Vdist;
            if (Math.Sign(bonbc) == 0 && BO.Length() <= BC.Length()) return t <= Vdist;

            return false;
        }

        /// <summary>
        /// Растояние от начала вектора до пересечения с плоскостью
        /// </summary>
        /// <param name="ray_view">Вектор</param>
        /// <returns>Расстояние</returns>
        public static float Get_dist_col(Ray ray_view)
        {
            //нормаль плоскости
            Vector3 N = new Vector3(0, 1, 0);
            //дистанция от начала координат
            float D = 0;
            float t1 = Vector3.Dot(N, ray_view.Direction);
            if (t1 == 0) t1 = 0.0000001f;
            float t = -(D + Vector3.Dot(N, ray_view.Position)) / t1;
            return t;
        }

        /// <summary>
        /// Растояние от начала вектора до пересечения с плоскостью
        /// </summary>
        /// <param name="ray_view">Вектор</param>
        /// <returns>Расстояние</returns>
        public static double Get_dist_col(double height, DVector3 Position, DVector3 Direction)
        {
            //нормаль плоскости
            DVector3 N = new DVector3(0, 1, 0);
            //дистанция от начала координат
            double D = height;
            double t1 = DVector3.Dot(N, Direction);
            if (t1 == 0) t1 = 0.0000001f;
            double t = -(D + DVector3.Dot(N, Position)) / t1;
            return t;
        }

        /// <summary>
        /// Растояние от начала вектора до пересечения с плоскостью
        /// </summary>
        /// <param name="height">растояние от начала кординат до плоскости</param>
        /// <param name="Position">Начало вектора</param>
        /// <param name="Direction">Направление</param>
        /// <param name="Normal">Нормаль плоскости</param>
        /// <returns></returns>
        public static double Get_dist_col(double height, DVector3 Position, DVector3 Direction, DVector3 Normal)
        {
            //нормаль плоскости
            //дистанция от начала координат
            double D = height;
            double t1 = DVector3.Dot(Normal, Direction);
            if (t1 == 0) t1 = 0.0000001f;
            double t = -(D + DVector3.Dot(Normal, Position)) / t1;
            return t;
        }

        /// <summary>
        /// Пересечение прямой и плоскости
        /// </summary>
        /// <param name="PosPlane">Точка плоскости</param>
        /// <param name="Normal">Нормаль</param>
        /// <param name="VPosition">Начало вектора</param>
        /// <param name="VDirection">Направление вектора</param>
        /// <param name="IPosition">Точка пересечения</param>
        /// <returns>Есть одна точка пересечения</returns>
        public static bool PlaneILine(DVector3 PosPlane, DVector3 Normal, DVector3 VPosition, DVector3 VDirection, out DVector3 IPosition)
        {
            IPosition = new DVector3();
            Normal.Normalize();
            DVector3 V = PosPlane - VPosition;
            double d = DVector3.Dot(Normal, V);
            double e = DVector3.Dot(Normal, VDirection);
            if (e != 0)
            {
                IPosition = VPosition + VDirection * d / e;
                return true;
            }
            return false;
        }

        /// <summary>
        /// Перевести значение в заданный интервал
        /// </summary>
        /// <param name="Input">Входное значение</param>
        /// <param name="InpMin">Нижняя граница входного значения</param>
        /// <param name="InpMax">Верхняя граница входного значения</param>
        /// <param name="Minm">Нижняя граница выходного</param>
        /// <param name="MaxRays">Верхняя граница выходного</param>
        /// <returns></returns>
        public static float RecalcInterval(float Input, float InpMin, float InpMax, float Min, float Max)
        {
            float Ug = 1 + (Input - InpMax) / (InpMax - InpMin);
            Ug = Ug * (Max - Min) + Min;
            if (Ug == float.NaN) Ug = Max;
            if (Min < Max)
            {
                if (Ug < Min) Ug = Min;
                if (Ug > Max) Ug = Max;
            }
            else
            {
                if (Ug > Min) Ug = Min;
                if (Ug < Max) Ug = Max;
            }
            return Ug;
        }

        /// <summary>
        /// Расчет нового размера массива с приведением к степени двух
        /// </summary>
        /// <param name="In"></param>
        /// <returns></returns>
        public static int GetBinResize(int In)
        {
            int n = (int)Math.Log((double)In, 2);
            if (Math.Pow(2, n) < In) n++;
            return (int)Math.Pow(2, n);
        }
    }
}